#include <pluginlib/class_list_macros.h>
#include <tf/LinearMath/Transform.h>
#include <tf/transform_datatypes.h>
#include "myplanner.h"
PLUGINLIB_EXPORT_CLASS(yyc_planner::MyLocalPlanner, nav_core::BaseLocalPlanner)
namespace yyc_planner{
MyLocalPlanner::MyLocalPlanner()
{

}
        
bool MyLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
{
    double cur_x = current_pose_.pose.position.x;
    double cur_y = current_pose_.pose.position.y;
    double cur_yaw = tf::getYaw(current_pose_.pose.orientation);
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();
    double goal_x = goal_pose.pose.position.x;
    double goal_y = goal_pose.pose.position.y;
    double goal_yaw  = tf::getYaw(goal_pose.pose.orientation);
    double dx = goal_x - cur_x;
    double dy = goal_y - cur_y;
    double target_yaw = atan2(dy, dx);
    double vx = 5.0*fabs(dx);
    double vy = 5.0*fabs(dy);
    double dtheta = target_yaw - cur_yaw;
    ROS_INFO("pose diff: %f, %f, %f", dx, dy, dtheta);
    double a = fmod(fmod(dtheta, 2.0 * M_PI) + 2.0 * M_PI, 2.0 * M_PI);
    if (a > M_PI) {
        a -= 2.0 * M_PI;
    }
    double wz = a;
    if(wz >  0.8)wz =  0.8;
    if(wz < -0.8)wz = -0.8;
    if(vx > 0.3)vx = 0.3;
    if(vy > 0.3)vy = 0.3;
    if(fabs(wz) > 0.05 && vx < 0.05)vx = 0.05;
    if(fabs(wz) > 0.05 && vy < 0.05)vy = 0.05;
    cmd_vel.linear.x = vx;
    cmd_vel.linear.y = vy;
    cmd_vel.angular.z = wz;
    ROS_INFO("cmd_vel: %f, %f, %f", vx, vy, wz);
    return true;
}
bool MyLocalPlanner::isGoalReached()
{
    if ( ! costmap_ros_->getRobotPose(current_pose_)) {
      ROS_ERROR("Could not get robot pose");
      return false;
    }
    double cur_x = current_pose_.pose.position.x;
    double cur_y = current_pose_.pose.position.y;
    geometry_msgs::PoseStamped goal_pose = global_plan_.back();
    double goal_x = goal_pose.pose.position.x;
    double goal_y = goal_pose.pose.position.y;
    if (hypot(goal_x - cur_x,goal_y - cur_y) <= 0.05)
    {
        ROS_INFO("Goal Reached");
        return true;
    }else{
        return false;
    }
}
bool MyLocalPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& plan)
{
    global_plan_.clear();
    global_plan_ = plan;
    return true;
}
void MyLocalPlanner::initialize( std::string name, 
                 tf2_ros::Buffer* tf, 
                 costmap_2d::Costmap2DROS* costmap_ros)
{
    tf_ = tf;
    costmap_ros_ = costmap_ros;
    costmap_ros_->getRobotPose(current_pose_);
}

}